#include <ColorSensor.h>
#include <DCMotor.h>
#include <Wire.h>

#define STATE_LF_COLOR     1
#define STATE_CTRL_COLOR   2
#define STATE_MOVE_FORWARD 3
#define STATE_MOVE_LEFT    4
#define STATE_MOVE_RIGHT   5
#define STATE_MOVE_STOP    6

int currentState;

Color color;
ColorSensor sensor(9, 5);
DCMotor motor(2,1,0);

void setup()
{ 
  currentState = STATE_LF_COLOR;
    
  sensor.begin();
  sensor.ledOn();
  motor.begin();
}

void loop()
{ 
  switch(currentState) {
    case STATE_LF_COLOR:
      motor.forward();
      color = sensor.read();
      if (color == _YELLOW)
      {
        currentState = STATE_CTRL_COLOR; 
      }
      break;
    case STATE_CTRL_COLOR:
      motor.forward();
      color = sensor.read();
      switch (color) {
        case _LILAC:
          currentState = STATE_MOVE_RIGHT;
          break;
        case _GREEN:
          currentState = STATE_MOVE_LEFT;
          break;
        case _RED:
          currentState = STATE_MOVE_STOP;
          break;
        default:
          break;
      }
      break;
    case STATE_MOVE_LEFT:
      motor.left();
      currentState = STATE_LF_COLOR;
      break;
    case STATE_MOVE_RIGHT:
      motor.right();
      currentState = STATE_LF_COLOR;
      break;
    case STATE_MOVE_STOP:
      motor.stop();
      break;
  }
}
